(in-package #:org.shirakumo.fraf.trial)

(defun describe-skeleton (skeleton stream)
  (let ((parents (parents (rest-pose skeleton)))
        (names (joint-names skeleton)))
    (flet ((bone-label (i)
             (format NIL "~3d ~@[~s~]" i (when (<= 0 i) (aref names i))))
           (bone-children (i)
             (loop for j from 0 below (length parents)
                   when (= (aref parents j) i) collect j)))
      (org.shirakumo.text-draw:tree -1 #'bone-children :key #'bone-label :stream stream :max-depth NIL))))

(defclass skeleton ()
  ((name :initarg :name :initform NIL :accessor name)
   (rest-pose :initarg :rest-pose :accessor rest-pose)
   (bind-pose :initarg :bind-pose :initform NIL :accessor bind-pose)
   (mat-inv-bind-pose :initform NIL :accessor mat-inv-bind-pose)
   (quat-inv-bind-pose :initform NIL :accessor quat-inv-bind-pose)
   (joint-names :initarg :joint-names :initform #() :accessor joint-names)
   (clips :initarg :clips :initform (make-hash-table :test 'equal) :accessor clips)))

(defmethod print-object ((skeleton skeleton) stream)
  (print-unreadable-object (skeleton stream :type T :identity T)
    (format stream "~@[~a~]" (name skeleton))))

(defmethod describe-object ((skeleton skeleton) stream)
  (call-next-method)
  (terpri stream)
  (describe-skeleton skeleton stream)
  (format stream "~&~%Clips:~%")
  (if (list-clips skeleton)
      (loop for clip in (list-clips skeleton)
            do (format stream "  ~s~%" clip))
      (format stream "  None~%")))

(defmethod shared-initialize :after ((skeleton skeleton) slots &key rest-pose)
  (when (bind-pose skeleton)
    (update-inv-bind-pose skeleton))
  (when rest-pose
    (setf (rest-pose skeleton) rest-pose)))

(define-table-accessor skeleton clip clips)

(defmethod stage :after ((skeleton skeleton) (area staging-area))
  (loop for clip being the hash-values of (clips skeleton)
        do (stage clip area)))

(defmethod (setf rest-pose) :after (pose (skeleton skeleton))
  (setf (joint-names skeleton) (%adjust-array (joint-names skeleton) (length pose))))

(defmethod (setf bind-pose) :after (pose (skeleton skeleton))
  (update-inv-bind-pose skeleton))

(defmethod node (name (skeleton skeleton))
  (position name (joint-names skeleton) :test #'equalp))

(defmethod update-inv-bind-pose ((skeleton skeleton))
  (let ((pose (bind-pose skeleton)))
    (let ((minv (make-array (length pose))))
      (dotimes (i (length minv) (setf (mat-inv-bind-pose skeleton) minv))
        (setf (svref minv i) (nminv (tmat (global-transform pose i))))))
    (let ((qinv (make-array (length pose))))
      (dotimes (i (length qinv) (setf (quat-inv-bind-pose skeleton) qinv))
        (setf (svref qinv i) (q2conjugate (global-quat2 pose i)))))
    skeleton))

(defmethod reorder ((skeleton skeleton) map)
  (let* ((rest (rest-pose skeleton))
         (bind (bind-pose skeleton))
         (names (joint-names skeleton))
         (forward (make-hash-table :test 'eql))
         (backward map)
         (hierarchy (make-array (length rest)))
         (process (make-array 0 :adjustable T :fill-pointer T)))
    (clrhash backward)
    (map-into hierarchy (lambda () (make-array 0 :element-type '(unsigned-byte 16) :adjustable T :fill-pointer T)))
    (dotimes (i (length rest))
      (let ((parent (parent-joint rest i)))
        (if (<= 0 parent)
            (vector-push-extend i (svref hierarchy parent))
            (vector-push-extend i process))))
    (loop for i from 0
          while (< i (length process))
          do (let* ((current (aref process i)))
               (loop for child across (svref hierarchy current)
                     do (vector-push-extend child process))
               (setf (gethash i forward) current)
               (setf (gethash current backward) i)))
    (setf (gethash -1 forward) -1)
    (setf (gethash -1 backward) -1)
    (let ((new-rest (make-instance 'pose :size (length rest)))
          (new-bind (make-instance 'pose :size (length rest)))
          (new-names (make-array (length rest))))
      (dotimes (i (length rest))
        (let* ((this (gethash i forward))
               (parent (gethash (parent-joint bind this) backward)))
          (setf (elt new-rest i) (elt rest this))
          (setf (elt new-bind i) (elt bind this))
          (setf (svref new-names i) (aref names this))
          (setf (parent-joint new-rest i) parent)
          (setf (parent-joint new-bind i) parent)))
      (check-consistent new-rest)
      (check-consistent new-bind)
      (setf (rest-pose skeleton) new-rest)
      (setf (bind-pose skeleton) new-bind)
      (setf (joint-names skeleton) new-names)
      skeleton)))

(defun instantiate-clip (skeleton clip &key (time (start-time clip)) (pose (rest-pose* skeleton)))
  (sample pose clip time)
  pose)

(defmethod rest-pose* ((skeleton skeleton) &rest initargs)
  (apply #'make-instance 'pose :source (rest-pose skeleton) initargs))
